#include <string.h>
#include <math.h>
#include "../control.h"
#include "joint_controller.h"
#include "cubic.h"
#include "quintic.h"
#include "../trajectory_plane/blendmath.h"
#include "../position_out.h"
#include "joint_queue.h"
#include "../limit_process.h"

extern STRUCT_CONTROL Ct;

//------------static api-----------------
static int jcComputeSegmentTime(double p0[MAX_JOINT], double p1[MAX_JOINT], float *segment_t);
static int jcInitialSegment(STRUCT_JOINT_CONTROL *jc);
static int jcAddCurrSegment(STRUCT_JOINT_CONTROL *jc);
static int jcComputeSegmentVel(double p[3][MAX_JOINT], float v[MAX_JOINT], float t[2]);

//************************************
// Method:    ��ʼ��joint controller
// FullName:  jcInit
// Access:    public 
// Returns:   void
// Qualifier:
//************************************
void jcInit(STRUCT_JOINT_CONTROL *jc)
{
	jcqCreate(&jc->jcq, JC_BUFF_LEN,jc->queue);
	return;
}

//************************************
// Method:    ����joint_control
// FullName:  jcClear
// Access:    public 
// Returns:   void
// Qualifier:
// Parameter: STRUCT_JOINT_CONTROL * jc
//************************************
void jcClear(STRUCT_JOINT_CONTROL *jc)
{
	jc->current_t = 0;
	jcqInit(&jc->jcq);
	return;
}


//************************************
// Method:    jcComputeSegmentTime
// FullName:  jcComputeSegmentTime
// Access:    public 
// Returns:   int 0: δ����,  1:�ٶ�����
// Qualifier:
// Parameter: STRUCT_JOINT_CONTROL * jc
//************************************
int jcComputeSegmentTime(double p0[MAX_JOINT],double p1[MAX_JOINT], float *segment_t)
{
	char over_speed_flag = 0;
	float joint_min_t[MAX_JOINT];
	int i,joint_min_t_mindex;
	for (i = 0; i < MAX_JOINT; i++){
		if (!Ct.conf.joint_enable[i]) joint_min_t[i] = 0;
		else{
			joint_min_t[i] = fabs(p1[i] - p0[i])
				/ Ct.conf.vel_joint.vel_max[i];
		}
	}
	joint_min_t_mindex = blFindMax(joint_min_t, MAX_JOINT);
	if (joint_min_t[joint_min_t_mindex] <= Ct.conf.inter_period){
		*segment_t = Ct.conf.inter_period;
	}
	else{
		*segment_t = joint_min_t[joint_min_t_mindex];
		over_speed_flag = 1;
	}
	if (over_speed_flag)  {
		DLog("over_speed,reset_t=%f\n", *segment_t);
		return -1;
	}
	else return 0;
}

//calc segment vel
int jcComputeSegmentVel(double p[3][MAX_JOINT], float v[MAX_JOINT], float t[2])
{
	int i;
	for (i = 0; i < MAX_JOINT; i++){
		v[i] = 0.5*((p[1][i] - p[0][i]) / t[0] + (p[2][i] - p[1][i]) / t[1]);
	}
	return 0;
}

int jcInitialSegment(STRUCT_JOINT_CONTROL *jc)
{
	int i = 0;
	jc->current_t = 0;
	STRUCT_JOINT_SEGMENT *curr_seg = NULL, *next_seg1 = NULL, *next_seg2 = NULL;
	float calc_t[2];
	curr_seg = jcqItem(&jc->jcq, 0);
	next_seg1 = jcqItem(&jc->jcq, 1);
	next_seg2 = jcqItem(&jc->jcq, 2);
	if (next_seg1 == NULL || curr_seg == NULL) {
		return -1;
	}
	memcpy(jc->p[0], curr_seg->posit, sizeof(double)*MAX_JOINT);
	memcpy(jc->p[1], next_seg1->posit, sizeof(double)*MAX_JOINT);
	jcComputeSegmentTime(jc->p[0], jc->p[1], &calc_t[0]);
	memcpy(jc->v[0], curr_seg->vl, sizeof(float)*MAX_JOINT);
	if (next_seg2 == NULL){
		memset(jc->v[1], 0, sizeof(float)*MAX_JOINT);
	}
	else{
		memcpy(jc->p[2], next_seg2->posit, sizeof(double)*MAX_JOINT);
		jcComputeSegmentTime(jc->p[1], jc->p[2], &calc_t[1]);
		jcComputeSegmentVel(jc->p, next_seg1->vl, calc_t); 
		memcpy(jc->v[1], next_seg1->vl, sizeof(float)*MAX_JOINT);
	}
	jc->segment_t = calc_t[0];
	//---------------------------
	for (i = 0; i < MAX_JOINT; i++){
		if (Ct.conf.joint_enable[i]){
			jc->cubic_para[i].posit[0] = jc->p[0][i];
			jc->cubic_para[i].posit[1] = jc->p[1][i];
			jc->cubic_para[i].vel[0] = jc->v[0][i];
			jc->cubic_para[i].vel[1] = jc->v[1][i];
			cbComputeFactor(&jc->cubic_para[i], &jc->cubic_factor[i],
				jc->segment_t);
		}
	}
#if 0
	DLog("joint,posit=%f %f %f,vel=%f %f",
		jc->p[0][0], jc->p[1][0], jc->p[2][0],
		jc->v[0][0], jc->v[1][0]
		);
#endif
	curr_seg->active = 1;
	return 0;
}


int jcAddCurrSegment(STRUCT_JOINT_CONTROL *jc)
{
	STRUCT_JOINT_SEGMENT curr_joint_segment;
	memcpy(curr_joint_segment.posit, Ct.curr_posit, sizeof(double)*MAX_JOINT);
	memset(curr_joint_segment.vl, 0, sizeof(float)*MAX_JOINT);
	curr_joint_segment.active = 0;
	jcqPut(&jc->jcq, &curr_joint_segment);
	return 0;
}

//************************************
// Method:    jcGetSegment
// FullName:  jcGetSegment
// Access:    public 
// Returns:   int
// Qualifier:
// Parameter: STRUCT_JOINT_CONTROL * jc
//************************************
int jcGetSegment(STRUCT_JOINT_CONTROL *jc)
{
	int i;
	STRUCT_JOINT_SEGMENT curr_joint_segment = {0};
	int jcq_len = jcqLen(&jc->jcq);
	for (i = jcq_len; i < 3; i++){
		if (Controller_GetNextPoint(curr_joint_segment.posit) != 0){
			//get faild
			break;
		}
		else{
			//get success
			if (jcqLen(&jc->jcq) == 0) jcAddCurrSegment(jc);
			curr_joint_segment.active = 0;
			jcqPut(&jc->jcq, &curr_joint_segment);
#if 0
			STRUCT_POSE pose;
			double *posit = curr_joint_segment.posit;
			Motion_KmClampForward(posit, &pose, NULL);
			fprintf(Ct.conf.fd1, "%.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f\n",
				posit[0], posit[1], posit[2], posit[3],
				pose.x, pose.y, pose.z, pose.rz, 0.0
				//posti[4],posti[5],posti[6],posti[7],posti[8]
				);
			fflush(Ct.conf.fd1);
#endif
		}
	}
	return 0;
}

//************************************
// Method:    jcProcessSegment
// FullName:  jcProcessSegment
// Access:    public 
// Returns:   int
// Qualifier:
// Parameter: STRUCT_JOINT_CONTROL * jc
//************************************
int jcProcessSegment(STRUCT_JOINT_CONTROL *jc)
{
	int i;
	STRUCT_JOINT_SEGMENT *curr_seg;
	STRUCT_PO_SEGMENT po_segment;
	jcGetSegment(jc);
	curr_seg = jcqItem(&jc->jcq,0);
	if (curr_seg == NULL){
		jcqInit(&jc->jcq);
		return 0;
	}
	if (!curr_seg->active){
		if (jcInitialSegment(jc) != 0) {
			jcqInit(&jc->jcq);
			return 0;
		}
	}
	jc->current_t += Ct.conf.joint_period;
	if (jc->current_t > jc->segment_t)jc->current_t = jc->segment_t;
	for (i = 0; i < MAX_JOINT; i++){
		if (Ct.conf.joint_enable[i]){
			cbComputePVA(&jc->cubic_factor[i], jc->current_t,
				&Ct.curr_posit[i],&Ct.curr_vel[i],&Ct.curr_acc[i]);
		}
	}
	memcpy(po_segment.posit, Ct.curr_posit, sizeof(double)*MAX_JOINT);
	memcpy(po_segment.vel, Ct.curr_vel, sizeof(float)*MAX_JOINT);
	memcpy(po_segment.acc, Ct.curr_acc, sizeof(float)*MAX_JOINT);
	//poqPut(&Ct.s_po.queue, &po_segment);
	Ct.conf.callback_out(po_segment.posit, po_segment.vel, po_segment.acc);	//直接回调输出不进入buff
	lmProcess(Ct.curr_posit, Ct.curr_vel, Ct.curr_acc);
	if (jc->current_t >= jc->segment_t){
		//excute end of a segment
		jcqRemove(&jc->jcq, 1);
	}
	return 0;
}

//************************************
// Method:    jcComputeRuncle
// FullName:  jcComputeRuncle
// Access:    public 
// Returns:   int
// Qualifier:
// Parameter: STRUCT_JOINT_CONTROL * jc
//************************************
int jcComputeRuncle(STRUCT_JOINT_CONTROL *jc)
{
	if (Ct.s_po.queue.allFull) return 0;
	jcProcessSegment(jc);
	return 0;
}

